/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <cmath>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cairo/cairo.h"
#include "slam/common/port.h"
#include "slam/io/image.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/id.h"
#include "slam/transform/rigid_transform.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/node_constants.h"
#include "slam_ros/ros_log_sink.h"
#include "slam_ros/submap.h"
#include "slam_ros/occupancy_grid_node.h"
#include "slam_ros_msgs/msg/submap_list.hpp"
#include "slam_ros_msgs/srv/submap_query.hpp"
#include "gflags/gflags.h"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "rclcpp/rclcpp.hpp"

DEFINE_double(resolution, 0.05,
              "Resolution of a grid cell in the published occupancy grid.");
DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
DEFINE_bool(include_frozen_submaps, true,
            "Include frozen submaps in the occupancy grid.");
DEFINE_bool(include_unfrozen_submaps, true,
            "Include unfrozen submaps in the occupancy grid.");
DEFINE_string(occupancy_grid_topic, slam_ros::kOccupancyGridTopic,
              "Name of the topic on which the occupancy grid is published.");
DEFINE_int32(pure_localization, 0, "Pure localization mode");  // 新增纯定位参数[1,2](@ref)

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps)
      << "Ignoring both frozen and unfrozen submaps makes no sense.";

  rclcpp::init(argc, argv);
  
  auto occupancy_grid_node = std::make_shared<slam_ros::OccupancyGridNode>(
      FLAGS_resolution,
      FLAGS_publish_period_sec,
      FLAGS_occupancy_grid_topic,
      FLAGS_include_frozen_submaps,
      FLAGS_pure_localization);

  slam_ros::ScopedRosLogSink ros_log_sink;

  rclcpp::spin(occupancy_grid_node);
  rclcpp::shutdown();
  return 0;
}